//
// Created by bit on 2022/8/9.
//

#include "guidance_filter/BlosSim.h"

void
BLOS_calc(const mavlink_local_position_ned_t &local_pos, const mavlink_attitude_quaternion_t &att_q,
            float N_target, float E_target, float D_target, float &vangle, float &hangle) {

    // 求目标在机体坐标系下的坐标
    // 坐标系1，以飞机为原点的NED坐标系
    // 坐标系2，机体坐标系
    // 坐标系w，世界坐标系
    Eigen::Quaterniond Q1 (1, 0, 0, 0);
    Eigen::Vector3d t1 (0, 0, 0);
    Eigen::Vector3d t2 (0, 0, 0);
    Eigen::Vector3d p1 (N_target - local_pos.x, E_target - local_pos.y, D_target - local_pos.z); // 点在1下的坐标
    Eigen::Vector3d p2;                            // 点在2下的坐标
    Eigen::Vector3d pw;                            // 点在世界坐标系下的坐标

    Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity();
    Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity();

//    std::cout << "receive q1 is " << att_q.q1 << ", receive q2 is " << att_q.q2 << ", receive q3 is " << att_q.q3 << std::endl;
    Eigen::Quaterniond Q2 (att_q.q1, att_q.q2, att_q.q3, att_q.q4);
    Q1.normalize();
    Q2.normalize();

    T_1w.rotate(Q1);
    T_1w.pretranslate(t1);
    T_2w.rotate(Q2);
    T_2w.pretranslate(t2);
    //求解P2即点在2下的坐标
    pw = T_1w * p1;
    p2 = T_2w.inverse() * pw;
//    std::cout << "该点在世界坐标系下坐标为" << pw.transpose() << std::endl;
//    std::cout << "该点在二号下的坐标为：" << p2.transpose() << std::endl;

    float dist_xy = pow(p2.transpose().x() * p2.transpose().x() + p2.transpose().y() * p2.transpose().y(), 0.5f);

    if (dist_xy > 0.5f) {
        vangle = atan2(-p2.transpose().z(), dist_xy);
        hangle = atan2(p2.transpose().y(), p2.transpose().x());
//        std::cout << "V " << vangle << ", H " << hangle << std::endl;
    } else {
        hangle = 0.0f;
        vangle = 0.0f;
    }
}

void
LOS_update(const mavlink_local_position_ned_t &vehicle,
           float N_target, float E_target, float D_target, float &vangle, float &hangle) {

//    float vel_angle = atan2(vehicle.vy, vehicle.vx);

    float N_relative = N_target - vehicle.x;
    float E_relative = E_target - vehicle.y;
    float D_relative = D_target - vehicle.z;

    float dist_xy = pow(N_relative * N_relative + E_relative * E_relative, 0.5f);

    float N_target_angle = atan2(E_relative, N_relative);

    // 即目标相对于飞机的方位角，与飞机姿态无关，只与两者相对位置有关
    // -180°到180度，正北向为0度，左转为正。
    float LOS_angle = N_target_angle;
    hangle = LOS_angle * 57.3f;
    hangle  = countRad(hangle);
    vangle = atan2(-D_relative, dist_xy) * 57.3f;
}

void
BLOS2LOS(const mavlink_attitude_quaternion_t &att, const LineOfSight &BLOS,
         float &LOS_vertical, float &LOS_horizon) {
    // 求目标在机体坐标系下的坐标
    // 坐标系1，以飞机为原点的NED坐标系，LOS
    // 坐标系2，相机坐标系
    // 坐标系w，世界坐标系

    Eigen::Quaterniond Q1 (1, 0, 0, 0);
//    Eigen::Quaterniond Q2 (1, 0, 0, 0);
//    Eigen::Quaterniond Q2 (-0.6284, -0.2505, 0.2229, 0.7019);   // west
//    Eigen::Quaterniond Q2 (-0.9422, -0.3344, -0.0202, -0.0077); // north
//    Eigen::Quaterniond Q2 (-0.707, 0, 0, -0.707); // east
//    Eigen::Quaterniond Q2 (0.0110, 0.0334, -0.3283, -0.9439);   // south
    Eigen::Quaterniond Q2 (att.q1, att.q2, att.q3, att.q4);
    Q1.normalize();
    Q2.normalize();

    Eigen::Vector3d t1 (0, 0, 0);
    Eigen::Vector3d p1; // 点在1下的坐标

//    Eigen::Isometry3d w_body = Eigen::Isometry3d::Identity();   // 机体质心NED坐标系到机体坐标系的转换
//    w_body.rotate(Q2);
//    w_body.pretranslate(t1);
//    Eigen::Vector3d t2 (0.406, 0, 0);
//    t2 = w_body * t2;
    Eigen::Vector3d t2 (0, 0, 0);
//    std::cout << "camera_pos x " << t2.x() << ", y " << t2.y() << ", z " << t2.z() << std::endl;

    Eigen::Vector3d p2 (cos(BLOS.horizontal / 57.3) * cos(BLOS.vertical / 57.3),
                        cos(BLOS.vertical / 57.3) * sin(BLOS.horizontal / 57.3),
                        -sin(BLOS.vertical / 57.3));                            // 点在2下的坐标
    Eigen::Vector3d pw;                            // 点在世界坐标系下的坐标

    Eigen::Isometry3d T_1w = Eigen::Isometry3d::Identity();
    Eigen::Isometry3d T_2w = Eigen::Isometry3d::Identity();
    T_1w.rotate(Q1);
    T_1w.pretranslate(t1);
    T_2w.rotate(Q2);
    T_2w.pretranslate(t2);

    //求解P2即点在2下的坐标
    pw = T_2w * p2;
    p1 = T_1w.inverse() * pw;
    pw.normalize();
    p1.normalize();
//    std::cout << "该点在世界坐标系下坐标为" << pw.transpose() << std::endl;
//    std::cout << "该点在LOS下的坐标为：" << p1.transpose() << std::endl;
//    std::cout << "该点在BLOS下的坐标为：" << p2.transpose() << std::endl;
//    std::cout << "pw: x " << p1.x() << ", y " << p1.y() << ", z " << p1.z() << std::endl;

    LOS_vertical = asinf(-p1.z());
    LOS_horizon = atan2f(p1.y(), p1.x());

    LOS_vertical *= 57.3;
    LOS_horizon *= 57.3;

    if (std::isnan(LOS_vertical) || std::isnan(LOS_horizon)) {
        std::cout << "BLOS transform to LOS error" <<  std::endl;
    }
}

float countRad(float a) {
    if (a > 180.0f) {
        a -= 180.0f * 2;
        return countRad(a);
    } else if (a < -180.0f) {
        a += 180.0f * 2;
        return countRad(a);
    } else {
        return a;
    }
}

void pos_to_BLOS(const UavStatus &px4_info,
                 const TargetInfo &target_pos,
                 LineOfSight &blos) {
    globallocalconverter_init(px4_info.home_pos.latitude * 1e-7,
                              px4_info.home_pos.longitude * 1e-7,
                              px4_info.home_pos.altitude * 1e-3,
                              currentTimeUs());

    mavlink_local_position_ned_t px4_local_pos;
    float N_target, E_target, D_target;

    globallocalconverter_tolocal(target_pos.lat, target_pos.lon, target_pos.alt,
                                 &N_target, &E_target, &D_target);

    globallocalconverter_tolocal(px4_info.globalPos.lat * 1e-7,
                                 px4_info.globalPos.lon * 1e-7,
                                 px4_info.globalPos.alt * 1e-3f,
                                 &px4_local_pos.x, &px4_local_pos.y, &px4_local_pos.z);

    BLOS_calc(px4_local_pos, px4_info.att_q,
              N_target, E_target, D_target, blos.vertical, blos.horizontal);
    blos.time_stamp_ms = currentTimeMs();
//    std::cout << "target, x " << N_target << ", y " << E_target
//    << "; vehicle, x " << px4_local_pos.x << ", y " << px4_local_pos.y;
//    std::cout << "; blos vertical " << blos.vertical << ", horizontal " << blos.horizontal << std::endl;

}